#include "algo_pid.h"

// PID参数初始化
void algo_pid_init(algo_pid_t *pid, f32 kp, f32 ki, f32 kd, f32 out_limit)
{
	pid->kp = kp;
	pid->ki = ki;
	pid->kd = kd;
	pid->out_limit = out_limit;
	
	pid->error = 0.0f;
	pid->last_error = 0.0f;
	pid->delta_error = 0.0f;
	pid->sum_error = 0.0f;
	pid->sum_err_ptr = 0;
	for (int i = 0; i < 10; ++i)
		pid->sum_error_temp[i] = 0.0f;
}

// PID输出计算
//u(k) = Kp*e(k) + Ki*∑e + Kd*Δe(k) , 式中Δe(k)=e(k)-e(k-1)
f32 algo_pid_calc(algo_pid_t *pid, f32 expc_value, f32 cur_value)
{
	f32 pid_out = 0.0f;

	pid->error = expc_value - cur_value;
	pid->delta_error = pid->error - pid->last_error;
	pid->last_error = pid->error;
	
	pid->sum_error_temp[pid->sum_err_ptr] =  pid->error;
	pid->sum_error = 0.0f;
	++(pid->sum_err_ptr);
	if (pid->sum_err_ptr >= 10)	pid->sum_err_ptr = 0;
	for (int i = 0; i < 10; ++i)
		pid->sum_error += pid->sum_error_temp[i];
	
	pid_out = pid->kp * pid->error + pid->ki * pid->sum_error + pid->kd * pid->delta_error;
	if (pid->out_limit != 0)
	{
		if (pid_out > pid->out_limit)	pid_out = pid->out_limit;
		else if (pid_out < -pid->out_limit)	pid_out = -pid->out_limit;
	}

	return pid_out;
}
